
from distutils.log import ERROR
import sys
import rospy
import moveit_commander
from math import pi
from moveit_commander.conversions import pose_to_list
import math

# from Message import HOME_JOINT,HOME_JOINT_7

def joint1(y,x):
    return math.degrees(math.atan(y/x))

def sqrt(a,b):
    return math.sqrt(a**2 + b**2)

def L2A(a,b,c):
        
    cosA = (b ** 2 + c ** 2 - a ** 2) / (2 * b * c)
    cosB = (a ** 2 + c ** 2 - b ** 2) / (2 * a * c)
    cosC = (a ** 2 + b ** 2 - c ** 2) / (2 * a * b)

    angleA = math.degrees(math.acos(cosA))
    angleB = math.degrees(math.acos(cosB))
    angleC = math.degrees(math.acos(cosC))

    return angleA,angleB,angleC

class FrankaPanda:
    def __init__(self):
        moveit_commander.roscpp_initialize(sys.argv)
        robot = moveit_commander.RobotCommander()
        group_name = "panda_arm"
        self.move_group = moveit_commander.MoveGroupCommander(group_name)

    def Go(self,jointlist):

        if len(jointlist) == 7:
            j1,j2,y1,j3,y2,j4,y3 = jointlist

        elif len(jointlist) == 4:
            j1,j2,j3,j4 = jointlist
            y1,y2,y3 = 0,0,0

        else:
            return ERROR

        joint_goal = self.move_group.get_current_joint_values()
        joint_goal[0] = math.radians(j1)
        joint_goal[1] = math.radians(j2)
        joint_goal[2] = math.radians(y1)
        joint_goal[3] = math.radians(j3)
        joint_goal[4] = math.radians(y2)
        joint_goal[5] = math.radians(j4)
        joint_goal[6] = math.radians(y3)

        self.move_group.go(joint_goal, wait=True)
        self.move_group.stop()

    def get_joint(self):
        joint= self.move_group.get_current_joint_values()
        return joint

    def get_end_pose(self):
        pose = self.move_group.get_current_pose().pose.position
        return pose

    def MoveTo(self,point):
        L1 = 0.333
        L2 = 0.316
        L3 = 0.384
        LendFront = 0.088
        LendHeight = 0.107 + 0.12
        E3 = 0.0825
        E2 = 0.0825
        L2F = sqrt(L2,E2)
        L3F = sqrt(L3,E3)

        
        x,y,z = point

        if sqrt(x,y) < 0.4:
            print("Too close to chassis,Now:",sqrt(x,y))
            return None

        j1 = joint1(y,x)

        ############
        if z + LendHeight > L1:
            dh = (z + LendHeight) - L1 
        else:
            dh = L1 - ( z + LendHeight )
        r = sqrt(x,y) - LendFront
        dist = sqrt(r,dh) 


        aL2F,aL3F,adist1 = L2A(L2F,L3F,dist)
        adh,ar,adist2 = L2A(dh,r,dist)
        aE3,aL3,alL3F = L2A(E3,L3,L3F)
        aE2,aL2,alL2F = L2A(E2,L2,L2F)


        if z + LendHeight > L1:
            j1 = joint1(y,x)
            j2 = 90 - adh - aL3F - aE2
            a = (360 - aL2 - aL3 - adist1)
            j4 = (180 - (180 - (360 - aL2 - aL3 - adist1)) ) * -1
            j7 = ar + aL2F + aE3
        else:
            j1 = joint1(y,x)
            j2 = 180 - (aE2 + aL3F + ar)
            a = 360 - aL2 - aL3 - adist1
            j4 = (180 - (180 - ( ))) * -1
            j7 = 90 + (adh + aL2F + aE3)
        
        return [j1,j2,j4,j7]

if __name__ == "__main__":
    rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
    franka = FrankaPanda()
    franka.Go(HOME_JOINT_7)
 
